package catchnthrow;

import se.lth.control.realtime.*;


public class Beam extends Thread {

	private AnalogIn inAngle;
	private AnalogOut outMotor;

	//Monitors
	private yBufferMonitor yBufMon;
	private static final int buffersize = 50;
	private isRunningMonitor isRunMon;
	private refMonitor refMon;
	
	private SM.uPlotMonitor uPlotMon;
	private final boolean plotU;

	private PID regul;


	/**
	 * Constructor
	 * @param inAngle analog input of current angle setting
	 * @param outMotor analog output to motor
	 * @param pri Priority of the thread
	 */
	public Beam(AnalogIn inAngle, AnalogOut outMotor,
			int pri) {
		setName("BeamControl"); //For debugging purposes
		this.inAngle = inAngle;
		this.outMotor = outMotor;
		setPriority(pri);
		plotU = false;

		yBufMon = new yBufferMonitor(buffersize);
		isRunMon = new isRunningMonitor(false);
		refMon = new refMonitor(0.0);
	}
	
	public Beam(AnalogIn inAngle, AnalogOut outMotor,
			SM.uPlotMonitor uMonitor, int pri) {
		setName("BeamControl"); //For debugging purposes
		this.inAngle = inAngle;
		this.outMotor = outMotor;
		setPriority(pri);
		plotU = true;

		yBufMon = new yBufferMonitor(buffersize);
		isRunMon = new isRunningMonitor(false);
		refMon = new refMonitor(0.0);
		uPlotMon = uMonitor;
	}

	/**
	 * Sets new reference
	 * @param ref
	 */
	public void setRef(double r){
		refMon.setRef(r);
	}

	public synchronized void setParameters(PIDParameters p) {
			if (regul == null) {
				regul = new PID("BeamControl", p);
			} else {
				regul.setParameters(p);
			}
		}

	/**
	 * Starts controlling loop until <code>shouldRun</code> is set to false
	 * @see stopControl()
	 */
	public void run() {
		int yIter = 0; //Iterator for yBuffer
		isRunMon.setShouldRun(true);
		long duration; 
		long t = System.currentTimeMillis();
		double u, yRef;
		double y = 0.0;

		try {
			y = inAngle.get();
		} catch (Exception x) {
		}

		while (true) {
			if(isRunMon.isShouldRun()){
				yRef = refMon.getRef();
				try {
					y = inAngle.get();
				} catch (Exception e) {
					e.printStackTrace();
				}
				u = regul.calculateOutput(y, yRef);
	
				u = limit(u, -10, 10);
	
				try {
					outMotor.set(u);
								
				} catch (Exception ex) {
					ex.printStackTrace();
				}
	
				regul.updateState(u);
				
				if(plotU) {
					uPlotMon.setU(u);
				}
	
				//Filling Buffer for tracing
				yBufMon.setYBufferEntry(yIter, y);
				if (yIter < buffersize-1) {
					yIter++;
				} else {
					yIter = 0;
				}
				
			}
			t = t + regul.getHMillis();
			duration = t - System.currentTimeMillis(); 
			if (duration > 0) { 
				try { 
					sleep(duration);
				} catch (Exception x) {} 
			} else {
			}
		}
	}

	/**
	 * Checks if current y is in an area with width 2*<code>error</code> around <code>yRef</code>
	 * @param error size of the area
	 * @return true if y is near yRef else false
	 */
	public boolean hasReached(double error) {
		double yMax = 0;
		double[] yBuffer = yBufMon.getYBuffer();
		double ref = refMon.getRef();
		
		for (int i = 0; i < buffersize; i++) {
			if (Math.abs(yBuffer[i] - ref) > yMax) {
				yMax = yBuffer[i];
			}
		}

		if (yMax <= ref+error && yMax >= ref-error) {
			return true;
		} else {
			return false;
		}
	}

	public boolean isRunning(){
		return isRunMon.isShouldRun();
	}
	
	public void restart(){
		isRunMon.setShouldRun(true);
	}
	

	/**
	 * Stops the controller loop
	 */
	public void shutDown(){
		isRunMon.setShouldRun(false);
		regul.reset();
		try {
			outMotor.set(0.0);
		} catch (Exception x) {
			x.printStackTrace();
		}
	}
	

	/**
	 * Saturation of v between min and max
	 * @param v
	 * @param min
	 * @param max
	 * @return saturated value
	 */
	private double limit(double v, double min, double max) {
		if (v < min) {
			return min;
		} else if (v > max) {
			return max;
		}
		return v;
	}

	private class yBufferMonitor{

		private double[] yBuffer;

		public yBufferMonitor(int size) {
			yBuffer = new double[size];
		}

		public synchronized double[] getYBuffer() {
			return yBuffer;
		}

		public synchronized void setYBuffer(double[] buffer) {
			yBuffer = buffer;
		}

		public synchronized void setYBufferEntry(int i, double entry) {
			yBuffer[i] = entry;
		}
	}

	private class isRunningMonitor{
		private boolean shouldRun;

		public isRunningMonitor(boolean init){
			shouldRun = init;
		}

		public synchronized boolean isShouldRun() {
			return shouldRun;
		}

		public synchronized void setShouldRun(boolean shouldRun) {
			this.shouldRun = shouldRun;
		}
	}

	private class refMonitor{
		private double ref;

		public refMonitor(double init){
			ref = init;
		}

		public synchronized double getRef() {
			return ref;
		}

		public synchronized void setRef(double ref) {
			this.ref = ref;
		}
	}
}
